import sensor, image, time
from machine import UART
import display
from pyb import UART
import math

# 颜色阈值
redblack_threshold = (3, 100, 26, 127, -128, 127)  # 胶带
redblack_threshold2 = (3, 100, 20, 127, -128, 127)
red_threshold = (16, 100, -128, 127, -128, 127)

# 初始化摄像头
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(200)
clock = time.clock()
sensor.set_auto_whitebal(False)  # 禁用白平衡
sensor.set_auto_gain(False)  # 关闭自动增益
sensor.set_auto_exposure(False, exposure_us=2000)  # 设置曝光度

# 全局变量
d = {}
mode = 0
last_mode=0
rx_buff = []
state = 0
res = 0
start = 0
get_point_from_rect = [0] * 8

# 初始化串口
uart = UART(3, 115200, timeout_char=1000)

# 初始化显示屏
lcd = display.SPIDisplay()

# 串口接收函数
def Receive_Prepare(da):
    global state, mode, start
    if state == 0 and da == 0x0d:  # 帧头
        state = 1
    elif state == 1:
        rx_buff.append(da)
        state = 2
    elif state == 2 and da == 0x0e:
        state = 0
        if rx_buff[0] == 0x23:
            mode=1
            start=0
        if rx_buff[0] == 0x24:
            mode=2
            start=0
        if rx_buff[0] == 0x25:
            start=1
        rx_buff.clear()
    else:
        state = 0
        rx_buff.clear()

# 矩形角点排序函数
def sort_corners(corners):
    corners = sorted(corners, key=lambda p: p[0] + p[1])  # 左上
    tl = corners[0]
    corners = sorted(corners[1:], key=lambda p: p[0] - p[1])  # 右上
    tr = corners[0]
    corners = sorted(corners[1:], key=lambda p: p[0] + p[1], reverse=True)  # 右下
    br = corners[0]
    bl = corners[1]  # 左下
    return [tl, tr, br, bl]

# 计算内框角点（基于向量偏移）
def get_inner_corners(corners, offset_ratio=0.1):
    # 计算矩形中心点
    cx = sum(p[0] for p in corners) // 4
    cy = sum(p[1] for p in corners) // 4

    # 每个角点向中心点偏移一定比例
    inner_corners = []
    for p in corners:
        dx = cx - p[0]
        dy = cy - p[1]
        new_x = p[0] + int(dx * offset_ratio)
        new_y = p[1] + int(dy * offset_ratio)
        inner_corners.append((new_x, new_y))
    return inner_corners

while True:
    img = sensor.snapshot()

    # 串口接收信息
    if uart.any() > 0:
        res = uart.readchar()
        Receive_Prepare(res)

    if last_mode != mode:
        last_mode=mode
        i=0
        while i<8:
            get_point_from_rect[i]=0
            i=i+1

    # 模式一：找矩形和红激光
    if mode == 2:
        sensor.set_windowing(100, 40, 140, 140)

        if start == 0:
            # 使用find_rects检测矩形
            rects = img.find_rects(threshold=10000, merge=10)
            if rects:
                best_rect = None
                max_area = 0
                for r in rects:
                    area = r.w() * r.h()
                    if area > max_area:
                        max_area = area
                        best_rect = r
                if best_rect:
                    # 获取矩形角点并排序
                    corners = sort_corners(best_rect.corners())
                    # 计算内框角点（向中心偏移15%）
                    inner_corners = get_inner_corners(corners, offset_ratio=0.1)
                    # 更新发送数据
                    for i, p in enumerate(inner_corners):
                        get_point_from_rect[i*2] = p[0]
                        get_point_from_rect[i*2+1] = p[1]
                    data = bytearray([0xB4, 0xB4] + [int(p) for p in get_point_from_rect] + [0x0D, 0x0A])
                    uart.write(data)
                    # 绘制内外框
                    img.draw_rectangle(best_rect.rect(), color=(0, 255, 0))  # 外框
                    for p in inner_corners:
                        # pass
                        img.draw_circle(p[0], p[1], 3, color=(255, 0, 0))  # 内框角点

        else:
            # 找红点逻辑
            redblack_blobs = img.find_blobs([redblack_threshold2], merge=20)
            if redblack_blobs:
                for a in redblack_blobs:
                    print(a[5], a[6])
                    img.draw_cross(a[5], a[6], color=(0, 0, 255))
                    data = bytearray([0xB3, 0xB3, a[5], a[6], 0x0D, 0x0A])
                    uart.write(data)

    if mode==1:
        sensor.set_windowing(50, 0, 240, 210)

        redblack_blobs = img.find_blobs([redblack_threshold2], merge=20)
        if redblack_blobs:
            for a in redblack_blobs:
                print(a[5], a[6])
                img.draw_cross(a[5], a[6], color=(0, 0, 255))
                data = bytearray([0xB3, 0xB3, a[5], a[6], 0x0D, 0x0A])
                uart.write(data)


    # 绘制当前存储的角点
    i = 0
    while i < 8:
        img.draw_circle(get_point_from_rect[i], get_point_from_rect[i+1], 3, color=(255, 0, 0))
        i += 2

    # 显示图像
    lcd.write(img)
    img.rotation_corr(z_rotation=180)  # 翻转图像
